/*
 *  V4L2 video capture example
 *
 * Copyright (C) 2022 Rockchip Electronics Co., Ltd.
 * Authors: 
 */
 
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
 
#include <getopt.h>             /* getopt_long() */
 
#include <fcntl.h>              /* low-level i/o */
#include <unistd.h>
#include <errno.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <linux/videodev2.h>

#include "camera.h"
 
#define CLEAR(x) memset(&(x), 0, sizeof(x))
 
#define errno_exit(s) \
{ \
        fprintf(stderr, "%s error %d, %s\n", s, errno, strerror(errno)); \
        return -1; \
}
 
static int xioctl(int fh, int request, void *arg)
{
        int r;
 
        do {
                r = ioctl(fh, request, arg);
        } while (-1 == r && EINTR == errno);
 
        return r;
}


int Camera::capture_image(const char *path)
{

        if (access(path, F_OK) == 0)
        {
                printf(" %s exists!\n", path);
                return -1;
        }

        capturfile_name = path;

        return 0;
}

int Camera::store_image(const void *data, int size)
{
        if (!capturfile_name)
        {
                return 0;
        }
        fflush(stderr);
        fprintf(stderr, ">");

        char file_name[64] = {0};
	FILE *fp = NULL;
        snprintf(file_name, sizeof(file_name),
             "%s", capturfile_name);
        fp = fopen(file_name, "ab+");
        if (fp == NULL) {
                printf("fopen yuv file %s failed!\n", file_name);
                return -1;
        }
        
        fwrite(data, size, 1, fp);
        printf("size %d \n", size);
        printf("Write success YUV data to %s \n",file_name);
        capturfile_name=NULL;
        fflush(fp);

        return 0;
}
 
void Camera::process_image(const void *data, int size)
{
        int index = frame_count % mq_max_len;
        cur_data_len = size;
        cur_data_start = (void *)data;

        store_image(data, size);
        mq_buffers[index].start = (void *)data;
        mq_buffers[index].length = size;
        mq.enqueue(mq_buffers[index]);
}

int Camera::read_frame(char *data, int max_len, int &end_size)
{
        struct buffer buffer;
        frame_sem.release();
        buffer = mq.dequeue();
        if (buffer.length < max_len)
        {
                memcpy(data, buffer.start, buffer.length);
                end_size = buffer.length;
                return 0;
        }

        return -1;
}
 
int Camera::read_frame(void)
{
        struct v4l2_buffer buf;
	struct v4l2_plane planes[FMT_NUM_PLANES];
	int bytesused;
		
        CLEAR(buf);

        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
        buf.memory = V4L2_MEMORY_MMAP;
                        // struct v4l2_plane planes[FMT_NUM_PLANES];
        memset(planes, 0, sizeof(struct v4l2_plane)*FMT_NUM_PLANES);
        if (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == buf.type) {
                buf.m.planes = planes;
                buf.length = FMT_NUM_PLANES;
        }
        if (-1 == xioctl(fd, VIDIOC_DQBUF, &buf)) {
                switch (errno) {
                case EAGAIN:
                        return 0;

                case EIO:
                        /* Could ignore EIO, see spec. */

                        /* fall through */

                default:
                        errno_exit("VIDIOC_DQBUF");
                }
        }
        if (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == buf.type)
                bytesused = buf.m.planes[0].bytesused;
        else
                bytesused = buf.bytesused;
        assert(buf.index < n_buffers);

        process_image(buffers[buf.index].start, bytesused);

        if (-1 == xioctl(fd, VIDIOC_QBUF, &buf))
                errno_exit("VIDIOC_QBUF");
 
        return 0;
}

//线程主体函数
bool Camera::threadLoop()
{
    
        frame_sem.acquire();
        for (;;) {
                fd_set fds;
                struct timeval tv;
                int r;


                FD_ZERO(&fds);
                FD_SET(fd, &fds);

                /* Timeout. */
                tv.tv_sec = 2;
                tv.tv_usec = 0;

                r = select(fd + 1, &fds, NULL, NULL, &tv);

                if (-1 == r) {
                        if (EINTR == errno)
                                continue;
                        return false;
                }

                if (0 == r) {
                        fprintf(stderr, "select timeout\n");
                        return false;
                }

                frame_count++;
                if (0 == read_frame())
                {
                        break;
                }
                /* EAGAIN - continue select loop. */
        }
                
        return true;
}
 
int Camera::stop_capturing(void)   
{
        enum v4l2_buf_type type;
 
        type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
        if (-1 == xioctl(fd, VIDIOC_STREAMOFF, &type))
                errno_exit("VIDIOC_STREAMOFF");
        return 0;
}
 
int Camera::start_capturing(void)
{
        unsigned int i;
        enum v4l2_buf_type type;
 
        for (i = 0; i < n_buffers; ++i) {
                struct v4l2_buffer buf;

                CLEAR(buf);
                buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
                buf.memory = V4L2_MEMORY_MMAP;
                buf.index = i;
                if (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == buf.type) {
                        struct v4l2_plane planes[FMT_NUM_PLANES];

                        buf.m.planes = planes;
                        buf.length = FMT_NUM_PLANES;
                }
                if (-1 == xioctl(fd, VIDIOC_QBUF, &buf))
                        errno_exit("VIDIOC_QBUF");
        }

        type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
        if (-1 == xioctl(fd, VIDIOC_STREAMON, &type))
                errno_exit("VIDIOC_STREAMON");
 
        run();

        return 0;
}
 
int Camera::uninit_device(void)
{
        unsigned int i;
 
 
        for (i = 0; i < n_buffers; ++i)
        {
                if (-1 == munmap(buffers[i].start, buffers[i].length))
                {
                        errno_exit("munmap");
                }
        }
                
        free(buffers);

        return 0;
}
 
int Camera::init_mmap(void)
{
        struct v4l2_requestbuffers req;
 
        CLEAR(req);
 
        req.count = 4;
        req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
        req.memory = V4L2_MEMORY_MMAP;
 
        if (-1 == xioctl(fd, VIDIOC_REQBUFS, &req)) {
                if (EINVAL == errno) {
                        fprintf(stderr, "%s does not support "
                                 "memory mappingn", dev_name);
                        return -1;
                } else {
                        errno_exit("VIDIOC_REQBUFS");
                }
        }
 
        if (req.count < 2) {
                fprintf(stderr, "Insufficient buffer memory on %s\n",
                         dev_name);
                return -1;
        }
 
        buffers = (struct buffer *)calloc(req.count, sizeof(struct buffer));
 
        if (!buffers) {
                fprintf(stderr, "Out of memory\n");
                return -1;
        }
 
        for (n_buffers = 0; n_buffers < req.count; ++n_buffers) { 
                struct v4l2_buffer buf;
		struct v4l2_plane planes[FMT_NUM_PLANES];
                CLEAR(buf);
	        CLEAR(planes);
                buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
                buf.memory      = V4L2_MEMORY_MMAP;
                buf.index       = n_buffers;
                if (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == buf.type) {
                        buf.m.planes = planes;
                        buf.length = FMT_NUM_PLANES;
                }
                if (-1 == xioctl(fd, VIDIOC_QUERYBUF, &buf))
                        errno_exit("VIDIOC_QUERYBUF");
 
                buffers[n_buffers].length = buf.m.planes[0].length;
                buffers[n_buffers].start =
                        mmap(NULL /* start anywhere */,
                              buf.m.planes[0].length,
                              PROT_READ | PROT_WRITE /* required */,
                              MAP_SHARED /* recommended */,
                              fd, buf.m.planes[0].m.mem_offset);
 
                if (MAP_FAILED == buffers[n_buffers].start)
                        errno_exit("mmap");
        }

        return 0;
}
 

 
int Camera::init_device(void)
{
        struct v4l2_capability cap;
        struct v4l2_format fmt;
        int ret;
 
        if (-1 == xioctl(fd, VIDIOC_QUERYCAP, &cap)) {
                if (EINVAL == errno) {
                        fprintf(stderr, "%s is no V4L2 device\n",
                                 dev_name);
                        exit(EXIT_FAILURE);
                } else {
                        errno_exit("VIDIOC_QUERYCAP");
                }
        }
 
        if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) &&
	    !(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE_MPLANE)) {
                fprintf(stderr, "%s is no video capture device\n",
                         dev_name);
                exit(EXIT_FAILURE);
        }
 
        
        if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
                fprintf(stderr, "%s does not support streaming i/o\n",
                                dev_name);
                exit(EXIT_FAILURE);
        }
                
        /* Select video input, video standard and tune here. */
        CLEAR(fmt);
 
        fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
        fmt.fmt.pix.width       = width;
        fmt.fmt.pix.height      = height;
        fmt.fmt.pix.pixelformat = format;
        fmt.fmt.pix.field       = V4L2_FIELD_NONE;
        if (-1 == xioctl(fd, VIDIOC_S_FMT, &fmt))
        {
                errno_exit("VIDIOC_S_FMT");
        }
 
        ret = init_mmap();

        return ret;

}
 
int Camera::cam_close(void)
{
        if (-1 == close(fd))
                errno_exit("close");
 
        fd = -1;

        return 0;
}
 
int Camera::cam_open(void)
{
        struct stat st;
 
        if (-1 == stat(dev_name, &st)) {
                fprintf(stderr, "Cannot identify '%s': %d, %s\n",
                         dev_name, errno, strerror(errno));
                exit(EXIT_FAILURE);
        }
 
        if (!S_ISCHR(st.st_mode)) {
                fprintf(stderr, "%s is no devicen", dev_name);
                exit(EXIT_FAILURE);
        }
 
        fd = open(dev_name, O_RDWR /* required */ | O_NONBLOCK, 0);
 
        if (-1 == fd) {
                fprintf(stderr, "Cannot open '%s': %d, %s\n",
                         dev_name, errno, strerror(errno));
                return -1;
        }

        return 0;
}

Camera::Camera(const char *device, int width, int height, int fmt)
:dev_name(device),
width(width),
height(height),
format(fmt),
capturfile_name(NULL),
frame_count(0)
{
        mq_max_len = 4;
        mq_buffers = (struct buffer *)calloc(mq_max_len, sizeof(struct buffer));
       cam_open();
       init_device();
       start_capturing();
}

Camera::~Camera()
{
        stop_capturing();
        uninit_device();
        cam_close();
        free(mq_buffers);
}
 
// static void usage(FILE *fp, int argc, char **argv)
// {
//         fprintf(fp,
//                  "Usage: %s [options]\n\n"
// 				 "argc: %d"
//                  "Version 1.3\n"
//                  "Options:\n"
//                  "-d | --device name   Video device name\n"
//                  "-p | --help          Print this message\n"
//                  "-w | --width         width of image\n"
//                  "-h | --height        height of image\n"
//                  "-m | --mmap          Use memory mapped buffers [default]\n"
//                  "-r | --read          Use read() calls\n"
//                  "-u | --userp         Use application allocated buffers\n"
//                  "-a | --dma           Use application allocated dma buffers\n"
//                  "-o | --output        Outputs stream to stdout\n"
//                  "-f | --format        Force format to YUYV\n"
//                  "-t | --stream-to     stream-to file default /data/dump_out.yuv\n"
//                  "-c | --count         Number of frames to grab\n"
//                  "-b | --test         test\n"
//                  "",
//                  argv[0], argc);
// }
 
// static const char short_options[] = "d:pw:h:mruaof:t:c:b";
 
// static const struct option
// long_options[] = {
//         { "device", required_argument, NULL, 'd' },
//         { "help",   no_argument,       NULL, 'p' },
//         { "width",   required_argument,       NULL, 'w' },
//         { "height",   required_argument,       NULL, 'h' },
//         { "mmap",   no_argument,       NULL, 'm' },
//         { "read",   no_argument,       NULL, 'r' },
//         { "userp",  no_argument,       NULL, 'u' },
//         { "dma",  no_argument,       NULL, 'a' },
//         { "output", no_argument,       NULL, 'o' },
//         { "format", required_argument,       NULL, 'f' },
//         { "stream-to", required_argument,       NULL, 't' },
//         { "count",  required_argument, NULL, 'c' },
//         { "test",  no_argument, NULL, 'b' },
//         { 0, 0, 0, 0 }
// };
 
// int main(int argc, char **argv)
// {
//         char *dev_name = NULL;
//         char *streamfile_name = NULL;

//         dev_name = (char *)"/dev/video0";
//         streamfile_name = (char *)"/home/dump_out.yuv";
 
//         for (;;) {
//                 int idx;
//                 int c;
 
//                 c = getopt_long(argc, argv,
//                                 short_options, long_options, &idx);
 
//                 if (-1 == c)
//                         break;
 
//                 switch (c) {
//                 case 0: /* getopt_long() flag */
//                         break;
 
//                 case 'd':
//                         dev_name = optarg;
//                         break;
 
//                 case 'p':
//                         usage(stdout, argc, argv);
//                         exit(EXIT_SUCCESS);
//                         break;
//                 default:
//                         usage(stderr, argc, argv);
//                         exit(EXIT_FAILURE);
//                 }
//         }
 
//         Camera cam("/dev/video0",1920,1080,V4L2_PIX_FMT_NV12);
//         cam.capture_image("/home/dump_out.yuv");
//         while(1);
//         fprintf(stderr, "\n");
//         return 0;
// }